import numpy as np
from scipy.spatial.transform import Rotation



def pick_box_r_l(box_width, central_pos, central_quat):

    central_rot = Rotation.from_quat([central_quat[1], central_quat[2], central_quat[3], central_quat[0]]).as_matrix()
    central_matrix = np.eye(4)
    central_matrix[:3, :3] = central_rot
    central_matrix[:3, 3] = central_pos

    cT_rd = np.array([[0, 0, -1, box_width/2],[0, 1, 0, 0],[1, 0, 0, 0],[0, 0, 0, 1]])
    cT_ld = np.array([[0, 0, 1, -box_width/2],[0, 1, 0, 0],[-1, 0, 0, 0],[0, 0, 0, 1]])

    rd_matrix = central_matrix@cT_rd
    rd_pos = rd_matrix[:3, 3]
    rd_quat = Rotation.from_matrix(rd_matrix[:3, :3]).as_quat()
    rd_quat = [rd_quat[3], rd_quat[0], rd_quat[1], rd_quat[2]]

    ld_matrix = central_matrix@cT_ld
    ld_pos = ld_matrix[:3, 3]
    ld_quat = Rotation.from_matrix(ld_matrix[:3, :3]).as_quat()
    ld_quat = [ld_quat[3], ld_quat[0], ld_quat[1], ld_quat[2]]
    return rd_pos, rd_quat, ld_pos, ld_quat
